#ifndef QUAT_EKF_H
#define QUAT_EKF_H

#include "main.h"
#include "kalman_filter.h"
#include "usb_task.h"

/* boolean type definitions */
#ifndef TRUE
#define TRUE 1 /**< boolean true  */
#endif

#ifndef FALSE
#define FALSE 0 /**< boolean fails */
#endif

typedef struct
{
    uint8_t Initialized;
    KalmanFilter_t IMU_QuaternionEKF;
    uint8_t ConvergeFlag;
    uint8_t StableFlag;
    uint64_t ErrorCount;
    uint64_t UpdateCount;

    fp32 GyroBias[3]; // 陀螺仪零偏估计值

    fp32 OrientationCosine[3];

    fp32 accLPFcoef;
    fp32 gyro_norm;
    fp32 accl_norm;
    fp32 AdaptiveGainScale;

    fp32 Q1; // 四元数更新过程噪声
    fp32 Q2; // 陀螺仪零偏过程噪声
    fp32 R;  // 加速度计量测噪声

    fp32 dt; // 姿态更新周期
    mat ChiSquare;
    fp32 ChiSquare_Data[1];      // 卡方检验检测函数
    fp32 ChiSquareTestThreshold; // 卡方检验阈值
    fp32 lambda;                 // 渐消因子
		
		int16_t YawRoundCount;
    fp32 last_yaw;
		
} QEKF_INS_t;

extern QEKF_INS_t QEKF_INS;
extern fp32 chiSquare;
extern fp32 ChiSquareTestThreshold;

/**
  * @brief          扩展卡尔曼滤波（Quaternion EKF）初始化和一些参考值
  * @param          process_noise1 四元数处理噪声 10
  * @param          process_noise2 陀螺仪偏移处理噪声 0.001
  * @param          measure_noise 加速度计测量噪声 1000
  * @param          lambda 衰减系数 0.9996
  * @param          lpf 低通滤波滤波器系数 0
  * @return         none
  */
extern void IMU_QuaternionEKF_Init(fp32 process_noise1, fp32 process_noise2, fp32 measure_noise, fp32 lambda, fp32 lpf);

/**
  * @brief          扩展卡尔曼滤波（Quaternion EKF）更新
  * @param          陀螺仪 x y z rad/s
  * @param          加速度计 x y z m/s²
  * @param          更新周期 s
  * @return         none
  */
extern void IMU_QuaternionEKF_Update(fp32 *INS_quat, fp32 *INS_gyro, fp32 *INS_accel, fp32 *INS_angle, fp32 dt);

#endif
